#ifndef LOCAL_NAVIGATOR_HPP
#define LOCAL_NAVIGATOR_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/string.hpp> 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tuple>
#include <memory>
#include <string>
#include <deque>
#include "local_navigation/center_json.hpp"
#include "local_navigation/vel_json.hpp"


namespace local_navigation{

    class LocalNavigator : public rclcpp::Node
    {
        public:

            explicit LocalNavigator(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

            ~LocalNavigator() override = default;

        private:

            // ROS 2 接口
            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr center_json_sub_;

            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_vel_pub_;
            rclcpp::TimerBase::SharedPtr control_timer_;

            // 状态变量
            double current_center_x_;
            double current_center_y_;
            double current_center_z_;
            bool target_detected_;
            int FILTER_WINDOW_SIZE_;

            // 精细调整参数
            double max_linear_velocity_;
            double position_tolerance_;
            double linear_x = 0.0;
            double linear_y = 0.0;
            double angular_z = 0.0;

            std::deque<double> x_buffer_;
            std::deque<double> y_buffer_;
            std::deque<double> z_buffer_;

            std::string center_json_topic_;
            std::string cmd_vel_topic_;

            CenterJsonConverter center_coords;
            VelocityJsonConverter vel_cmd;


            // 回调函数
            void centerJsonCallback(const std_msgs::msg::String::SharedPtr msg);
            void controlLoop();

            // 速度计算函数
            void calculateVelocityFromDistance(double& linear_x, double& linear_y, double& angular_z, double avg_x, double avg_y );

    };

}

#endif